/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include "motor.h"

motor_t motor_create(unsigned long size)
{
    motor_t new_motor = (motor_t)malloc(size);
    if (new_motor == NULL)
    {
        aos_cli_printf("Falied to allocate memory for new motor\n");
        return NULL;
    }

    return new_motor;
}

unsigned long motor_enable(motor_t mot)
{
    CHECK_RET_WITH_RET(mot, 0);
    // Enable PWM
    aos_cli_printf("Enabling motor");
    mot->enable(mot);

    return 1;
}

unsigned long motor_disable(motor_t mot)
{
    CHECK_RET_WITH_RET(mot, 0);

    // Disable PWM
    aos_cli_printf("Disabling motor");
    mot->disable(mot);

    return 1;
}

unsigned long motor_run(motor_t mot, signed short thousands)
{
    CHECK_RET_WITH_RET(mot, 0);

    // Set speed (pwm) to desired value
    mot->set_speed(mot, thousands);

    return 1;
}

unsigned long motor_stop(motor_t mot)
{
    CHECK_RET_WITH_RET(mot, 0);

    // Set Speed to 0
    motor_run(mot, 0);

    return 1;
}

unsigned long motor_destroy(motor_t mot)
{
    CHECK_RET_WITH_RET(mot, 0);
    
    // Disable first
    motor_disable(mot);

    // Free memory
    aos_cli_printf("Free motor");

    free(mot);

    return 1;
}

unsigned long motor_reset(motor_t mot)
{
    CHECK_RET_WITH_RET(mot, 0);

    return mot->reset(mot);
}
